% desired command sequence
% goToWaypoint [40.437042, -79.948652, 0.5]
% goToWaypoint [40.436650, -79.948925, 0.5]
% goToWaypoint [40.436819, -79.948472, 0.5]
% goToWaypoint [40.437042, -79.948652, 0.5]
% halt

boat = 1;
sendCommand(boat, 'goToWaypoint', 40.437042, -79.948652, 0.5);
pause(3); % Waiting for 3 secs to ensure that command has time to be sent and parsed

stat = getStatus(boat);
while (stat.started == 1)
    pause(1);
    stat = getStatus(boat);
end

sendCommand(boat, 'goToWaypoint', 40.436650, -79.948925, 0.5);
pause(3); % Waiting for 3 secs to ensure that command has time to be sent and parsed

stat = getStatus(boat);
while (stat.started == 1)
    pause(1);
    stat = getStatus(boat);
end

sendCommand(boat, 'goToWaypoint', 40.436819, -79.948472, 0.5);
pause(3); % Waiting for 3 secs to ensure that command has time to be sent and parsed

stat = getStatus(boat);
while (stat.started == 1)
    pause(1);
    stat = getStatus(boat);
end

sendCommand(boat, 'goToWaypoint', 40.437042, -79.948652, 0.5);
pause(3); % Waiting for 3 secs to ensure that command has time to be sent and parsed

stat = getStatus(boat);
while (stat.started == 1)
    pause(1);
    stat = getStatus(boat);
end

sendCommand(boat, 'halt');
